#include <cv_bridge/cv_bridge.h>
#include <glog/logging.h>
#include <image_transport/image_transport.h>
#include <message_filters/subscriber.h>
#include <message_filters/sync_policies/approximate_time.h>
#include <message_filters/synchronizer.h>
#include <ros/ros.h>
#include <sensor_msgs/Image.h>
#include <signal.h>
#include <chrono>
#include <opencv2/opencv.hpp>
#include <thread>
#include "commons/rabbit_consts.h"
#include "commons/x264_encoder.h"
#include "commons/libar8020.h"

using namespace cv;

void MySigintHandler(int sig) {
  ROS_INFO("shutting down!");
  ros::shutdown();
  exit(0);
}
void videoFuse(Mat &big,Mat &small)
{
   Rect roi = Rect(0,0, small.cols, small.rows);
   Mat subView = big(roi); 
   small.copyTo(subView);
}
int main(int argc, char** argv) {

  // init ros
  ros::init(argc, argv, "video_fuse_node");
  ros::Time::init();
  ros::NodeHandle nh;
  while (Usb_Init_So() < 0) {        //图传模块初始化
    LOG(INFO) << "libar8020 usb init error, reinit in 5 secs";
    std::this_thread::sleep_for(std::chrono::seconds(5));
  }
  signal(SIGINT, MySigintHandler);
  DLOG(INFO) << "Init Ros";
  // create filter callback
  auto callback_fn = [&](const sensor_msgs::ImageConstPtr& before,
                         const sensor_msgs::ImageConstPtr& after) {
    x264Encoder endocer(BIG_VIDEO_HIGHT, BIG_VIDEO_WIDTH, 3, 15);
    cv::Mat frame_before = cv_bridge::toCvShare(after, "bgr8")->image;                   
    cv::Mat frame_after = cv_bridge::toCvShare(before, "bgr8")->image;
     cv::resize(frame_before, frame_before, cv::Size(SMALL_VIDEO_HIGHT, SMALL_VIDEO_WIDTH));
    videoFuse(frame_after,frame_before);  //small to big
    int size = endocer.EncodeOneFrame(frame_after);
    uchar* data = nullptr;
    int res = -1;
    data = endocer.GetEncodedFrame();
    try {
      res = Video_Port_Send_So((char*)data, size);
      std::cout<< "Send h264 with res " << res<< "\r\n"; 
      //LOG(INFO) << "Send h264 with res " << res;
     } catch (...) {
       // LOG(INFO) << "Error send h264";
    }
  // cv::imshow("view_fuse", frame_after);          
        
    cv::waitKey(20);
    frame_before.release();
    frame_after.release();
    

  };
  // read data from camera node and model prediction node
  message_filters::Subscriber<sensor_msgs::Image> before_sub(
      nh, TOPIC_CAMERA, 1);
  message_filters::Subscriber<sensor_msgs::Image> after_sub(
      nh, TOPIC_CAMERA_TWO, 1);
  typedef message_filters::sync_policies::ApproximateTime<
      sensor_msgs::Image, sensor_msgs::Image>
      MySyncPolicy;
  message_filters::Synchronizer<MySyncPolicy> sync(MySyncPolicy(100), before_sub,
                                                   after_sub);
  sync.registerCallback(boost::bind<void>(callback_fn, _1, _2));
  ros::spin();
  return 0;
}
